//Random number generator in the range [-1,1]
float randomNumberGenerator(int x, int y){
	int n = (int)(x * 40.0 + y * 6400.0);
	n = (n << 13) ^ n;
	return 1.0f - (float)( (n * (n * n * 15731 + 789221) + 1376312589) & 0x7fffffff) / 1073741824.0f;
}

//Performs the seek behavior for each particle
float4 seekKernel(float4 target, float4 pos, float4 vel, float maxVel){
	float slowingDistance = 2;
	float4 target_offset = (float4)(target.x - pos.x, 0.0f, target.z - pos.z, 0.0f);
	float distance = length(target_offset);
	if(distance > 0.0f){
		target_offset = normalize(target_offset);
		if(distance < slowingDistance)
			target_offset *= maxVel*(distance/slowingDistance);
		else
			target_offset *= maxVel;
		return (target_offset - vel);
	}else{
		return (float4)(0.0f, 0.0f, 0.0f, 0.0f);
	}
}

//Performs the wander behavior for each particle
float4 wanderKernel(float4 pos, float4 vel, float wanderTheta, float maxVel){
	float wanderRadius = 4.0f;
	float wanderDistance = 5.0f;
	
	float4 circleLocation = normalize(vel);
	circleLocation *= wanderDistance;
	circleLocation += pos;
	
	float4 target = (float4)(wanderRadius * native_cos(wanderTheta), 0.0f, wanderRadius * native_sin(wanderTheta), 0.0f);
	target += circleLocation;
	
	return seekKernel(target, pos, vel, maxVel);
}

//Performs the flocking behavior for each particle
float4 flockKernel(	global float4 *agents,
					float4 pos,
					global float4 *velocity,
					float4 vel,
					global float2 *originalID,
					int vision,
					int desiredSeparation,
					int alignDistance,
					float maxSpeed,
					float maxForce,
					int radius, 
					int matrixPosX, 
					int matrixPosY,
					int agents_mesh_width){
					
	int separationCount = 0;
	int alignmentCount = 0;
	int cohesionCount = 0;
	
	float4 separation = (float4) 0;
	float4 alignment = (float4) 0;
	float4 cohesion = (float4) 0;
	
	//Top left corner
	int topLeftX = matrixPosX - radius;
	int topLeftY = matrixPosY - radius;
	
	//Bottom Right corner
	int botRightX = matrixPosX + radius;
	int botRightY = matrixPosY + radius;
	
	//Check matrix boundaries
	topLeftX = max(0, topLeftX);
	topLeftY = max(0, topLeftY);
	botRightX = min(botRightX, agents_mesh_width);
	botRightY = min(botRightY, agents_mesh_width);
	
	for(int i=topLeftY; i<=botRightY; i++){
		for(int j=topLeftX; j<=botRightX; j++){
			if(i == matrixPosY && j == matrixPosX)
				continue;
				
			float4 neighbor = agents[i*agents_mesh_width + j];
			float2 neighborID = originalID[i*agents_mesh_width + j];
			float distance = length(neighbor - pos);
			//Separation
			if(distance > 0 && distance < desiredSeparation){
				float4 diff = pos - neighbor;
				diff = normalize(diff);
				diff *= 1/distance;
				separation += diff;
				separationCount++;
			}
			//Alignment
			if(distance > 0 && distance < alignDistance){
				float4 rawNeighborVel = velocity[(int)(neighborID.y * agents_mesh_width + neighborID.x)];
				float4 neighborVel = (rawNeighborVel.x, rawNeighborVel.y, rawNeighborVel.z, 0.0f);
				alignment += neighborVel;
				alignmentCount++;
			}
			//Cohesion
			if(distance > desiredSeparation && distance < alignDistance){
				cohesion += neighbor;
				cohesionCount++;
			}
		}
	}
	
	if(separationCount > 0)
		separation *= 1/(float)separationCount;
		
	if(alignmentCount > 0)
		alignment *= 1/(float)alignmentCount;
		
	if(cohesionCount > 0){
		cohesion *= 1/(float)cohesionCount;
		cohesion = seekKernel(cohesion, pos, vel, maxSpeed); 
	}
		
	if(length(alignment) > 0){
		alignment = normalize(alignment);
		alignment *= maxSpeed;
		alignment -= vel;
		alignment = clamp(alignment, -maxForce, maxForce); 
	}
		
	return 2.0 * separation + alignment;
}

// Moves the agents with basic flock rules
kernel void basicMovementKernel(global float4 *agents,
								global float4 *velocity,
								global float *maxVelocity,
								global float *maxAcceleration,
								global float4* groupingOptions,
								global float4* movementOptions,
								global float4 *message,
								global float2 *originalID,
								int agents_mesh_width,
								float time){
	
	unsigned int x = get_global_id(0);
	unsigned int y = get_global_id(1);
	
	if( x * y > agents_mesh_width * agents_mesh_width )
		return;
	
	float4 pos = agents  [y * agents_mesh_width + x];
	float2 posID = originalID[y * agents_mesh_width + x];
	
	float4 velocityAndDirection = velocity[(int)(posID.y * agents_mesh_width + posID.x)];
	
	float4 vel = (float4)(velocityAndDirection.x, velocityAndDirection.y, velocityAndDirection.z, 0.0f);
	float wanderTheta = velocityAndDirection.w;
	
	float maxVel = maxVelocity[(int)(posID.y * agents_mesh_width + posID.x)];
	float maxAccel = maxAcceleration[(int)(posID.y * agents_mesh_width + posID.x)];
	
	float4 groupOptions = groupingOptions[(int)(posID.y * agents_mesh_width + posID.x)];
	
	float isLeader = groupOptions.x;
	float myGroup = groupOptions.y;
	
	float4 seek = (float4) 0;
	float4 wander = (float4) 0;
	float4 flock = (float4) 0;
	
	if(myGroup != -1 ){
		if( isLeader == 1.0f ){
			wanderTheta += 0.25 * randomNumberGenerator(posID.x, posID.y);
			wander = wanderKernel(pos, vel, wanderTheta, maxVel);
		}else{
			float4 target =  message[(int)myGroup];
			seek = seekKernel(target, pos, vel, maxVel);
		}
		flock = flockKernel(agents, pos, velocity, vel, originalID, 0, 6, 12, maxVel, maxAccel, 1, x, y, agents_mesh_width);
	}
	
	if( isLeader == 1.0f ){
		vel += 0.7 * flock + 0.3 * wander;
	}else{
		vel += 0.7 * flock + 0.3 * seek;
	}
	
	pos += vel*time;
	
	agents  [y*agents_mesh_width + x] = pos;
	velocity[(int)(posID.y * agents_mesh_width + posID.x)] = (float4)(vel.x, vel.y, vel.z, wanderTheta);
}